﻿//===========================[ 6 / 19 / 2011 LVI ]=============================
// 								Source file
// Purpose: MathLib combination 2 source mathematics libs
// [1] dhpoware : http://www.dhpoware.com/source/mathlib.html
// [3] math lib from NVIDIA Graphics SDK
// Modified by Vitaly Lyaschenko <SkyChameleon@gmail.com>
//=============================================================================
#include "mathlib.h"

//=========================================================================== 
//									FUNCTIONS
//=========================================================================== 

//--------------------------
// FUNCTIONS 2D Vector
//--------------------------

float distance(const vec2 &pt1, const vec2 &pt2)
{
	// Calculates the distance between 2 points.
	return sqrtf(distanceSq(pt1, pt2));
}

float distanceSq(const vec2 &pt1, const vec2 &pt2)
{
	// Calculates the squared distance between 2 points.
	return ((pt1.x - pt2.x) * (pt1.x - pt2.x))
		+ ((pt1.y - pt2.y) * (pt1.y - pt2.y));
}

float dot(const vec2 &p, const vec2 &q)
{
	return (p.x * q.x) + (p.y * q.y);
}

vec2 lerp(const vec2 &p, const vec2 &q, float t)
{
	// Linearly interpolates from 'p' to 'q' as t varies from 0 to 1.
	return p + t * (q - p);
}

void orthogonalize(vec2 &v1, vec2 &v2)
{
	// Performs Gram-Schmidt Orthogonalization on the 2 basis vectors to
	// turn them into orthonormal basis vectors.
	v2 = v2 - proj(v2, v1);
	normalize(v2);
}

vec2 proj(const vec2 &p, const vec2 &q)
{
	// Calculates the projection of 'p' onto 'q'.
	float len = length(q);
	return (dot(p, q) / (len * len)) * q;
}

vec2 perp(const vec2 &p, const vec2 &q)
{
	// Calculates the component of 'p' perpendicular to 'q'.
	float len = length(q);
	return p - ((dot(p, q) / (len * len)) * q);
}

vec2 reflect(const vec2 &i, const vec2 &n)
{
	// Calculates reflection vector from entering ray direction 'i'
	// and surface normal 'n'.
	return i - 2.0f * proj(i, n);
}

float length(const vec2 &v)
{
	return sqrtf((v.x * v.x) + (v.y * v.y));
}

float lengthSq(const vec2 &v)
{
	return (v.x * v.x) + (v.y * v.y);
}

vec2 invert(const vec2 v)
{
	return vec2(-v.x, -v.y);
}

void normalize(vec2 &v)
{
	float invMag = 1.0f / length(v);
	v.x *= invMag, v.y *= invMag;
}

void set(vec2 &v, float x_, float y_)
{
	v.x = x_, v.y = y_;
}

//--------------------------
// FUNCTIONS 3D Vector
//--------------------------

vec3 cross(const vec3 &p, const vec3 &q)
{
	return vec3((p.y * q.z) - (p.z * q.y),
				(p.z * q.x) - (p.x * q.z),
				(p.x * q.y) - (p.y * q.x));
}

float distance(const vec3 &pt1, const vec3 &pt2)
{
	// Calculates the distance between 2 points.
	return sqrtf(distanceSq(pt1, pt2));
}

float distanceSq(const vec3 &pt1, const vec3 &pt2)
{
	// Calculates the squared distance between 2 points.
	return ((pt1.x - pt2.x) * (pt1.x - pt2.x))
		 + ((pt1.y - pt2.y) * (pt1.y - pt2.y))
		 + ((pt1.z - pt2.z) * (pt1.z - pt2.z));
}

float dot(const vec3 &p, const vec3 &q)
{
	return (p.x * q.x) + (p.y * q.y) + (p.z * q.z);
}

vec3 lerp(const vec3 &p, const vec3 &q, float t)
{
	// Linearly interpolates from 'p' to 'q' as t varies from 0 to 1.
	return p + t * (q - p);
}

void orthogonalize(vec3 &v1, vec3 &v2)
{
	// Performs Gram-Schmidt Orthogonalization on the 2 basis vectors to
	// turn them into orthonormal basis vectors.
	v2 = v2 - proj(v2, v1);
	normalize(v2);
}

void orthogonalize(vec3 &v1, vec3 &v2, vec3 &v3)
{
	// Performs Gram-Schmidt Orthogonalization on the 3 basis vectors to
	// turn them into orthonormal basis vectors.

	v2 = v2 - proj(v2, v1);
	normalize(v2);

	v3 = v3 - proj(v3, v1) - proj(v3, v2);
	normalize(v3);
}

vec3 proj( vec3 &p,  vec3 &q)
{
	// Calculates the projection of 'p' onto 'q'.
	float len =  length(q);
	return (dot(p, q) / (len * len)) * q;
}

vec3 perp(vec3 &p, vec3 &q)
{
	// Calculates the component of 'p' perpendicular to 'q'.
	float len = length(q);
	return p - ((dot(p, q) / (len * len)) * q);
}

vec3 reflect(vec3 &i, vec3 &n)
{
	// Calculates reflection vector from entering ray direction 'i'
	// and surface normal 'n'.
	return i - 2.0f * proj(i, n);
}

float length(const vec3 &v)
{
	return sqrtf(dot(v,v));
}

float lengthSq(vec3 &v)
{
	return (v.x * v.x) + (v.y * v.y) + (v.z * v.z);
}

vec3 invert(const vec3 v)
{
	return vec3(-v.x, -v.y, -v.z);
}

vec3 normalize(const vec3 &v)
{
	vec3 v_ = v;

	float invMag = 1.0f / length(v_);
	v_.x *= invMag, v_.y *= invMag, v_.z *= invMag;

	return v_;
}

void normalize(vec3 &v)
{
	float invMag = 1.0f / length(v);
	v.x *= invMag, v.y *= invMag, v.z *= invMag;
}

void set(vec3 &v, float x_, float y_, float z_)
{ 
	v.x = x_, v.y = y_, v.z = z_; 
}

//--------------------------
// FUNCTIONS 4D Vector
//--------------------------

float distance(const vec4 &pt1, const vec4 &pt2)
{
	// Calculates the distance between 2 points.
	return sqrtf(distanceSq(pt1, pt2));
}

float distanceSq(const vec4 &pt1, const vec4 &pt2)
{
	// Calculates the squared distance between 2 points.
	return ((pt1.x - pt2.x) * (pt1.x - pt2.x))
		+ ((pt1.y - pt2.y) * (pt1.y - pt2.y))
		+ ((pt1.z - pt2.z) * (pt1.z - pt2.z))
		+ ((pt1.w - pt2.w) * (pt1.w - pt2.w));
}

float dot(const vec4 &p, const vec4 &q)
{
	return (p.x * q.x) + (p.y * q.y) + (p.z * q.z) + (p.w * q.w);
}

vec4 lerp(const vec4 &p, const vec4 &q, float t)
{
	// Linearly interpolates from 'p' to 'q' as t varies from 0 to 1.
	return p + t * (q - p);
}

float length(const vec4 v)
{
	return sqrtf((v.x * v.x) + (v.y * v.y) + (v.z * v.z) + (v.w * v.w));
}

float lengthSq(const vec4 v)
{
	return (v.x * v.x) + (v.y * v.y) + (v.z * v.z) + (v.w * v.w);
}

vec4 invert(const vec4 v)
{
	return vec4(-v.x, -v.y, -v.z, -v.w);
}

void normalize(vec4 &v)
{
	float invMag = 1.0f / length(v);
	v.x *= invMag, v.y *= invMag, v.z *= invMag, v.w *= invMag;
}

void set(vec4 &v, float x_, float y_, float z_, float w_)
{ 
	v.x = x_, v.y = y_, v.z = z_, v.w = w_; 
}

//--------------------------
// FUNCTIONS row-major 3x3 MATRIX
//--------------------------

float determinant(mat3 M)
{
	return (M.m_arr[0] * (M.m_arr[4] * M.m_arr[5] - M.m_arr[7] * M.m_arr[8]))
		 - (M.m_arr[4] * (M.m_arr[1] * M.m_arr[2] - M.m_arr[7] * M.m_arr[8]))
		 + (M.m_arr[6] * (M.m_arr[1] * M.m_arr[2] - M.m_arr[4] * M.m_arr[5]));
}

void fromAxes(mat3 &M, const vec3 &x, const vec3 &y, const vec3 &z)
{
	M.m_arr[0] = x.x,  M.m_arr[1] = x.y,  M.m_arr[2] = x.z;
	M.m_arr[3] = y.x,  M.m_arr[4] = y.y,  M.m_arr[5] = y.z;
	M.m_arr[6] = z.x,  M.m_arr[7] = z.y,  M.m_arr[8] = z.z;
}

void fromAxesTransposed(mat3 &M, const vec3 &x, const vec3 &y, const vec3 &z)
{
	M.m_arr[0] = x.x,  M.m_arr[1] = y.x,  M.m_arr[2] = z.x;
	M.m_arr[3] = x.y,  M.m_arr[4] = y.y,  M.m_arr[5] = z.y;
	M.m_arr[6] = x.z,  M.m_arr[7] = y.z,  M.m_arr[8] = z.z;
}

void identity(mat3 &M)
{
	M.m_arr[0] = 1.0f, M.m_arr[1] = 0.0f, M.m_arr[2] = 0.0f;
	M.m_arr[3] = 0.0f, M.m_arr[4] = 1.0f, M.m_arr[5] = 0.0f;
	M.m_arr[6] = 0.0f, M.m_arr[7] = 0.0f, M.m_arr[8] = 1.0f;
}

mat3 invert(const mat3& A)
{
	mat3 B;
	float det,oodet;

	det = determinant(A);

	if ( closeEnough(det, 0.0f))
	{
		identity(B);
		return B;
	}

	oodet = 1.0f / det;

	B.a0 *= oodet; B.a1 *= oodet; B.a2 *= oodet;
	B.a3 *= oodet; B.a4 *= oodet; B.a5 *= oodet;
	B.a6 *= oodet; B.a7 *= oodet; B.a8 *= oodet;

	return B;
}

void rotate(mat3 &M, const vec3 &axis, float degrees)
{
	// Creates a rotation matrix about the specified axis.
	// The axis must be a unit vector. The angle must be in degrees.
	//
	// Let u = axis of rotation = (x, y, z)
	//
	//             | x^2(1 - c) + c  xy(1 - c) + zs  xz(1 - c) - ys |
	// Ru(angle) = | yx(1 - c) - zs  y^2(1 - c) + c  yz(1 - c) + xs |
	//             | zx(1 - c) - ys  zy(1 - c) - xs  z^2(1 - c) + c |
	//
	// where,
	//	c = cos(angle)
	//  s = sin(angle)

	degrees = degreesToRadians(degrees);

	float x = axis.x;
	float y = axis.y;
	float z = axis.z;
	float c = cosf(degrees);
	float s = sinf(degrees);

	M.m_arr[0] = (x * x) * (1.0f - c) + c;
	M.m_arr[1] = (x * y) * (1.0f - c) + (z * s);
	M.m_arr[2] = (x * z) * (1.0f - c) - (y * s);

	M.m_arr[4] = (y * x) * (1.0f - c) - (z * s);
	M.m_arr[5] = (y * y) * (1.0f - c) + c;
	M.m_arr[6] = (y * z) * (1.0f - c) + (x * s);

	M.m_arr[ 8] = (z * x) * (1.0f - c) + (y * s);
	M.m_arr[ 9] = (z * y) * (1.0f - c) - (x * s);
	M.m_arr[10] = (z * z) * (1.0f - c) + c;

}

void scale(mat3 &M, float sx, float sy, float sz)
{
	// Creates a scaling matrix.
	//
	//                 | sx   0    0  |
	// S(sx, sy, sz) = | 0    sy   0  |
	//                 | 0    0    sz |

	M.m_arr[0] = sx,   M.m_arr[1] = 0.0f, M.m_arr[ 2] = 0.0f;
	M.m_arr[4] = 0.0f, M.m_arr[5] = sy,   M.m_arr[ 6] = 0.0f;
	M.m_arr[8] = 0.0f, M.m_arr[9] = 0.0f, M.m_arr[10] = sz;

}

void scale(mat3 &M, const vec3 v)
{
	scale(M, v.x, v.y, v.z);
}

void toAxes(mat3 &M, vec3 &x, vec3 &y, vec3 &z)
{
	set(x, M.m_arr[0], M.m_arr[1], M.m_arr[ 2]);
	set(y, M.m_arr[4], M.m_arr[5], M.m_arr[ 6]);
	set(z, M.m_arr[8], M.m_arr[9], M.m_arr[10]);

}

void toAxesTransposed(mat3 &M, vec3 &x, vec3 &y, vec3 &z)
{
	set(x, M.m_arr[0], M.m_arr[4], M.m_arr[ 8]);
	set(y, M.m_arr[1], M.m_arr[5], M.m_arr[ 9]);
	set(z, M.m_arr[2], M.m_arr[6], M.m_arr[10]);
}

mat3 transpose(mat3 M)
{
	mat3 tmp;

	tmp.m_arr[0] = M.m_arr[0], tmp.m_arr[1] = M.m_arr[4], tmp.m_arr[ 2] = M.m_arr[ 8];
	tmp.m_arr[4] = M.m_arr[1], tmp.m_arr[5] = M.m_arr[5], tmp.m_arr[ 6] = M.m_arr[ 9];
	tmp.m_arr[8] = M.m_arr[2], tmp.m_arr[6] = M.m_arr[6], tmp.m_arr[10] = M.m_arr[10];

	return tmp;
}

//--------------------------
// FUNCTIONS row-major 4x4 MATRIX
//--------------------------

float determinant(mat4 M)
{
	return (M.m_arr[ 0] * M.m_arr[ 5] - M.m_arr[ 4] * M.m_arr[ 1])
		 * (M.m_arr[10] * M.m_arr[15] - M.m_arr[14] * M.m_arr[11])
	     - (M.m_arr[ 0] * M.m_arr[ 9] - M.m_arr[ 8] * M.m_arr[ 1])
		 * (M.m_arr[ 6] * M.m_arr[15] - M.m_arr[14] * M.m_arr[ 7])
		 + (M.m_arr[ 0] * M.m_arr[13] - M.m_arr[12] * M.m_arr[ 1])
		 * (M.m_arr[ 6] * M.m_arr[11] - M.m_arr[10] * M.m_arr[ 7])
		 + (M.m_arr[ 4] * M.m_arr[ 9] - M.m_arr[ 8] * M.m_arr[ 5])
		 * (M.m_arr[ 2] * M.m_arr[15] - M.m_arr[14] * M.m_arr[ 3])
		 - (M.m_arr[ 4] * M.m_arr[13] - M.m_arr[12] * M.m_arr[ 5])
		 * (M.m_arr[ 2] * M.m_arr[11] - M.m_arr[10] * M.m_arr[ 3])
		 + (M.m_arr[ 8] * M.m_arr[13] - M.m_arr[12] * M.m_arr[ 9])
		 * (M.m_arr[ 2] * M.m_arr[ 7] - M.m_arr[ 6] * M.m_arr[ 3]);
}


void fromAxes(mat4 &M, const vec3 &x, const vec3 &y, const vec3 &z)
{

	M.m_arr[ 0] = x.x,  M.m_arr[ 1] = x.y,  M.m_arr[ 2] = x.z,  M.m_arr[ 3] = 0.0f;
	M.m_arr[ 4] = y.x,  M.m_arr[ 5] = y.y,  M.m_arr[ 6] = y.z,  M.m_arr[ 7] = 0.0f;
	M.m_arr[ 8] = z.x,  M.m_arr[ 9] = z.y,  M.m_arr[10] = z.z,  M.m_arr[11] = 0.0f;
	M.m_arr[12] = 0.0f, M.m_arr[13] = 0.0f, M.m_arr[14] = 0.0f, M.m_arr[15] = 1.0f;

}

void fromAxesTransposed(mat4 &M, const vec3 &x, const vec3 &y, const vec3 &z)
{

	M.m_arr[ 0] = x.x,  M.m_arr[ 1] = y.x,  M.m_arr[ 2] = z.x,  M.m_arr[ 3] = 0.0f;
	M.m_arr[ 4] = x.y,  M.m_arr[ 5] = y.y,  M.m_arr[ 6] = z.y,  M.m_arr[ 7] = 0.0f;
	M.m_arr[ 8] = x.z,  M.m_arr[ 9] = y.z,  M.m_arr[10] = z.z,  M.m_arr[11] = 0.0f;
	M.m_arr[12] = 0.0f, M.m_arr[13] = 0.0f, M.m_arr[14] = 0.0f, M.m_arr[15] = 1.0f;

}

void identity(mat4 &M)
{
	M.m_arr[ 0] = 1.0f, M.m_arr[ 1] = 0.0f, M.m_arr[ 2] = 0.0f, M.m_arr[ 3] = 0.0f;
	M.m_arr[ 4] = 0.0f, M.m_arr[ 5] = 1.0f, M.m_arr[ 6] = 0.0f, M.m_arr[ 7] = 0.0f;
	M.m_arr[ 8] = 0.0f, M.m_arr[ 9] = 0.0f, M.m_arr[10] = 1.0f, M.m_arr[11] = 0.0f;
	M.m_arr[12] = 0.0f, M.m_arr[13] = 0.0f, M.m_arr[14] = 0.0f, M.m_arr[15] = 1.0f;
}

void toAxes(mat4 &M, vec3 &x, vec3 &y,vec3 &z)
{
	set(x, M.m_arr[0], M.m_arr[1], M.m_arr[ 2]);
	set(y, M.m_arr[4], M.m_arr[5], M.m_arr[ 6]);
	set(z, M.m_arr[8], M.m_arr[9], M.m_arr[10]);
}

mat4 &invert(mat4& B, const mat4& M)
{
	float det,oodet;
	
	det = determinant(M);
	// если детерминант достаточно близок к 
	if ( closeEnough(det, 0.0f))
	{
		identity(B);
		return B;
	}

	oodet = 1.0f / det;

	B.a0  *= oodet;
	B.a4  *= oodet;
	B.a8  *= oodet;
	B.a12 *= oodet;

	B.a1  *= oodet;
	B.a5  *= oodet;
	B.a7  *= oodet;
	B.a13 *= oodet;

	B.a2  *= oodet;
	B.a6  *= oodet;
	B.a10 *= oodet;
	B.a14 *= oodet;

	B.a3  *= oodet;
	B.a7  *= oodet;
	B.a11 *= oodet;
	B.a15 *= oodet;

	return B;
}

mat4 invert_rot_trans( const mat4& M )
{
	mat4 B;

	B.a0  = M.a0;
	B.a4  = M.a1;
	B.a8  = M.a2;
	B.a12 = M.a12;
	B.a1  = M.a4;
	B.a5  = M.a5;
	B.a9  = M.a6;
	B.a13 = M.a13;
	B.a2  = M.a8;
	B.a6  = M.a9;
	B.a10 = M.a10;
	B.a14 = M.a14;
	B.a3  = -(M.a0 * M.a3 + M.a4 * M.a7 + M.a8  * M.a11);
	B.a7  = -(M.a1 * M.a3 + M.a5 * M.a7 + M.a9  * M.a11);
	B.a11 = -(M.a2 * M.a3 + M.a6 * M.a7 + M.a10 * M.a11);
	B.a15 = M.a15;

	return B;
}

void orient(mat4 &M, const vec3 &from, const vec3 &to)
{
	// Creates an orientation matrix that will rotate the vector 'from' 
	// into the vector 'to'. For this method to work correctly, vector
	// 'from' and vector 'to' must both be unit length vectors.
	//
	// The algorithm used is from:
	//   Tomas Moller and John F. Hughes, "Efficiently building a matrix
	//   to rotate one vector to another," Journal of Graphics Tools,
	//   4(4):1-4, 1999.

	float e = dot(from, to);

	if ( closeEnough(e, 1.0f))
	{
		// Special case where 'from' is equal to 'to'. In other words,
		// the angle between vector 'from' and vector 'to' is zero 
		// degrees. In this case just load the identity matrix.

		identity(M);
	}
	else if ( closeEnough(e, -1.0f))
	{
		// Special case where 'from' is directly opposite to 'to'. In
		// other words, the angle between vector 'from' and vector 'to'
		// is 180 degrees. In this case, the following matrix is used:
		//
		// Let:
		//   F = from
		//   S = vector perpendicular to F
		//   U = S X F
		//
		// We want to rotate from (F, U, S) to (-F, U, -S)
		//
		// | -FxFx+UxUx-SxSx  -FxFy+UxUy-SxSy  -FxFz+UxUz-SxSz  0 |
		// | -FxFy+UxUy-SxSy  -FyFy+UyUy-SySy  -FyFz+UyUz-SySz  0 |
		// | -FxFz+UxUz-SxSz  -FyFz+UyUz-SySz  -FzFz+UzUz-SzSz  0 |
		// |       0                 0                0         1 |

		vec3 side(0.0f, from.z, -from.y);

		if ( closeEnough(dot(side, side), 0.0f))
			set(side, -from.z, 0.0f, from.x);

		normalize(side);

		vec3 up = cross(side, from);
		normalize(up);

		M.m_arr[ 0] = -(from.x * from.x) + (up.x * up.x) - (side.x * side.x);
		M.m_arr[ 1] = -(from.x * from.y) + (up.x * up.y) - (side.x * side.y);
		M.m_arr[ 2] = -(from.x * from.z) + (up.x * up.z) - (side.x * side.z);
		M.m_arr[ 3] = 0.0f;
		M.m_arr[ 4] = -(from.x * from.y) + (up.x * up.y) - (side.x * side.y);
		M.m_arr[ 5] = -(from.y * from.y) + (up.y * up.y) - (side.y * side.y);
		M.m_arr[ 6] = -(from.y * from.z) + (up.y * up.z) - (side.y * side.z);
		M.m_arr[ 7] = 0.0f;
		M.m_arr[ 8] = -(from.x * from.z) + (up.x * up.z) - (side.x * side.z);
		M.m_arr[ 9] = -(from.y * from.z) + (up.y * up.z) - (side.y * side.z);
		M.m_arr[10] = -(from.z * from.z) + (up.z * up.z) - (side.z * side.z);
		M.m_arr[11] = 0.0f;
		M.m_arr[12] = 0.0f;
		M.m_arr[13] = 0.0f;
		M.m_arr[14] = 0.0f;
		M.m_arr[15] = 1.0f;
	}
	else
	{
		// This is the most common case. Creates the rotation matrix:
		//
		//               | E + HVx^2   HVxVy + Vz  HVxVz - Vy  0 |
		// R(from, to) = | HVxVy - Vz  E + HVy^2   HVxVz + Vx  0 |
		//               | HVxVz + Vy  HVyVz - Vx  E + HVz^2   0 |
		//               |     0           0           0       1 |
		//
		// where,
		//   V = from.cross(to)
		//   E = from.dot(to)
		//   H = (1 - E) / V.dot(V)

		vec3 v = cross(from, to);
		normalize(v);

		float h = (1.0f - e) / dot(v, v);

		M.m_arr[0] = e + h * v.x * v.x;
		M.m_arr[1] = h * v.x * v.y + v.z;
		M.m_arr[2] = h * v.x * v.z - v.y;
		M.m_arr[3] = 0.0f;

		M.m_arr[4] = h * v.x * v.y - v.z;
		M.m_arr[5] = e + h * v.y * v.y;
		M.m_arr[6] = h * v.x * v.z + v.x;
		M.m_arr[7] = 0.0f;

		M.m_arr[ 8] = h * v.x * v.z + v.y;
		M.m_arr[ 9] = h * v.y * v.z - v.x;
		M.m_arr[10] = e + h * v.z * v.z;
		M.m_arr[11] = 0.0f;

		M.m_arr[12] = 0.0f;
		M.m_arr[13] = 0.0f;
		M.m_arr[14] = 0.0f;
		M.m_arr[15] = 1.0f;
	}

}
mat4 rotate( const vec3 &axis, float degrees)
{
	mat4 M;
	// Creates a rotation matrix about the specified axis.
	// The axis must be a unit vector. The angle must be in degrees.
	//
	// Let u = axis of rotation = (x, y, z)
	//
	//             | x^2(1 - c) + c  xy(1 - c) + zs  xz(1 - c) - ys   0 |
	// Ru(angle) = | yx(1 - c) - zs  y^2(1 - c) + c  yz(1 - c) + xs   0 |
	//             | zx(1 - c) - ys  zy(1 - c) - xs  z^2(1 - c) + c   0 |
	//             |      0              0                0           1 |
	//
	// where,
	//	c = cos(angle)
	//  s = sin(angle)

	degrees = degreesToRadians(degrees);

	float x = axis.x;
	float y = axis.y;
	float z = axis.z;
	float c = cosf(degrees);
	float s = sinf(degrees);

	M.m_arr[0] = (x * x) * (1.0f - c) + c;
	M.m_arr[1] = (x * y) * (1.0f - c) + (z * s);
	M.m_arr[2] = (x * z) * (1.0f - c) - (y * s);
	M.m_arr[3] = 0.0f;

	M.m_arr[4] = (y * x) * (1.0f - c) - (z * s);
	M.m_arr[5] = (y * y) * (1.0f - c) + c;
	M.m_arr[6] = (y * z) * (1.0f - c) + (x * s);
	M.m_arr[7] = 0.0f;

	M.m_arr[ 8] = (z * x) * (1.0f - c) + (y * s);
	M.m_arr[ 9] = (z * y) * (1.0f - c) - (x * s);
	M.m_arr[10] = (z * z) * (1.0f - c) + c;
	M.m_arr[11] = 0.0f;

	M.m_arr[12] = 0.0f;
	M.m_arr[13] = 0.0f;
	M.m_arr[14] = 0.0f;
	M.m_arr[15] = 1.0f;

	return M;
}

void rotate( mat4 &M, const vec3 &axis, float degrees )
{
	// Creates a rotation matrix about the specified axis.
	// The axis must be a unit vector. The angle must be in degrees.
	//
	// Let u = axis of rotation = (x, y, z)
	//
	//             | x^2(1 - c) + c  xy(1 - c) + zs  xz(1 - c) - ys   0 |
	// Ru(angle) = | yx(1 - c) - zs  y^2(1 - c) + c  yz(1 - c) + xs   0 |
	//             | zx(1 - c) - ys  zy(1 - c) - xs  z^2(1 - c) + c   0 |
	//             |      0              0                0           1 |
	//
	// where,
	//	c = cos(angle)
	//  s = sin(angle)

	degrees = degreesToRadians(degrees);

	float x = axis.x;
	float y = axis.y;
	float z = axis.z;
	float c = cosf(degrees);
	float s = sinf(degrees);

	M.m_arr[0] = (x * x) * (1.0f - c) + c;
	M.m_arr[1] = (x * y) * (1.0f - c) + (z * s);
	M.m_arr[2] = (x * z) * (1.0f - c) - (y * s);
	M.m_arr[3] = 0.0f;

	M.m_arr[4] = (y * x) * (1.0f - c) - (z * s);
	M.m_arr[5] = (y * y) * (1.0f - c) + c;
	M.m_arr[6] = (y * z) * (1.0f - c) + (x * s);
	M.m_arr[7] = 0.0f;

	M.m_arr[ 8] = (z * x) * (1.0f - c) + (y * s);
	M.m_arr[ 9] = (z * y) * (1.0f - c) - (x * s);
	M.m_arr[10] = (z * z) * (1.0f - c) + c;
	M.m_arr[11] = 0.0f;

	M.m_arr[12] = 0.0f;
	M.m_arr[13] = 0.0f;
	M.m_arr[14] = 0.0f;
	M.m_arr[15] = 1.0f;

	//return M;
}

void rotateX( mat4 &M, float angle ) 
{
	float c = cos(angle);
	float s = sin(angle);
	M.m_arr[0] = 1.0; M.m_arr[4] = 0.0; M.m_arr[ 8] = 0.0; M.m_arr[12] = 0.0;
	M.m_arr[1] = 0.0; M.m_arr[5] = c;   M.m_arr[ 9] = -s;  M.m_arr[13] = 0.0;
	M.m_arr[2] = 0.0; M.m_arr[6] = s;   M.m_arr[10] = c;   M.m_arr[14] = 0.0;
	M.m_arr[3] = 0.0; M.m_arr[7] = 0.0; M.m_arr[11] = 0.0; M.m_arr[15] = 1.0;
}

mat4 rotateX( float angle ) 
{
	mat4 M;
	float c = cos(angle);
	float s = sin(angle);
	M.m_arr[0] = 1.0; M.m_arr[4] = 0.0; M.m_arr[ 8] = 0.0; M.m_arr[12] = 0.0;
	M.m_arr[1] = 0.0; M.m_arr[5] = c;   M.m_arr[ 9] = -s;  M.m_arr[13] = 0.0;
	M.m_arr[2] = 0.0; M.m_arr[6] = s;   M.m_arr[10] = c;   M.m_arr[14] = 0.0;
	M.m_arr[3] = 0.0; M.m_arr[7] = 0.0; M.m_arr[11] = 0.0; M.m_arr[15] = 1.0;
	
	return M;
}

void rotateY( mat4 &M, float angle ) 
{			
	float c = cos(angle);
	float s = sin(angle);
	M.m_arr[0] = c;   M.m_arr[4] = 0.0; M.m_arr[ 8] = s;   M.m_arr[12] = 0.0;
	M.m_arr[1] = 0.0; M.m_arr[5] = 1.0; M.m_arr[ 9] = 0.0; M.m_arr[13] = 0.0;
	M.m_arr[2] = -s;  M.m_arr[6] = 0.0; M.m_arr[10] = c;   M.m_arr[14] = 0.0;
	M.m_arr[3] = 0.0; M.m_arr[7] = 0.0; M.m_arr[11] = 0.0; M.m_arr[15] = 1.0;
}

mat4 rotateY(  float angle ) 
{	
	mat4 M;
	float c = cos(angle);
	float s = sin(angle);
	M.m_arr[0] = c;   M.m_arr[4] = 0.0; M.m_arr[ 8] = s;   M.m_arr[12] = 0.0;
	M.m_arr[1] = 0.0; M.m_arr[5] = 1.0; M.m_arr[ 9] = 0.0; M.m_arr[13] = 0.0;
	M.m_arr[2] = -s;  M.m_arr[6] = 0.0; M.m_arr[10] = c;   M.m_arr[14] = 0.0;
	M.m_arr[3] = 0.0; M.m_arr[7] = 0.0; M.m_arr[11] = 0.0; M.m_arr[15] = 1.0;

	return M;
}

void rotateZ( mat4 &M, float angle ) 
{	
	float c = cos(angle);
	float s = sin(angle);
	M.m_arr[0] = c;   M.m_arr[4] = -s;  M.m_arr[8] = 0.0;  M.m_arr[12] = 0.0;
	M.m_arr[1] = s;   M.m_arr[5] = c;   M.m_arr[9] = 0.0;  M.m_arr[13] = 0.0;
	M.m_arr[2] = 0.0; M.m_arr[6] = 0.0; M.m_arr[10] = 1.0; M.m_arr[14] = 0.0;
	M.m_arr[3] = 0.0; M.m_arr[7] = 0.0; M.m_arr[11] = 0.0; M.m_arr[15] = 1.0;
}

mat4 rotateZ( float angle ) 
{	
	mat4 M;
	float c = cos(angle);
	float s = sin(angle);
	M.m_arr[0] = c;   M.m_arr[4] = -s;  M.m_arr[8] = 0.0;  M.m_arr[12] = 0.0;
	M.m_arr[1] = s;   M.m_arr[5] = c;   M.m_arr[9] = 0.0;  M.m_arr[13] = 0.0;
	M.m_arr[2] = 0.0; M.m_arr[6] = 0.0; M.m_arr[10] = 1.0; M.m_arr[14] = 0.0;
	M.m_arr[3] = 0.0; M.m_arr[7] = 0.0; M.m_arr[11] = 0.0; M.m_arr[15] = 1.0;

	return M;
}

mat4 scale(float sx, float sy, float sz)
{
	// Creates a scaling matrix.
	//
	//                 | sx   0    0    0 |
	// S(sx, sy, sz) = | 0    sy   0    0 |
	//                 | 0    0    sz   0 |
	//                 | 0    0    0    1 |

	mat4 M;

	M.m_arr[ 0] = sx,   M.m_arr[ 1] = 0.0f, M.m_arr[ 2] = 0.0f, M.m_arr[ 3] = 0.0f;
	M.m_arr[ 4] = 0.0f, M.m_arr[ 5] = sy,   M.m_arr[ 6] = 0.0f, M.m_arr[ 7] = 0.0f;
	M.m_arr[ 8] = 0.0f, M.m_arr[ 9] = 0.0f, M.m_arr[10] = sz,   M.m_arr[11] = 0.0f;
	M.m_arr[12] = 0.0f, M.m_arr[13] = 0.0f, M.m_arr[14] = 0.0f, M.m_arr[15] = 1.0f;

	return M;
}

mat4 scale(vec3 v)
{
	return scale(v.x, v.y, v.z);
}

void toAxesTransposed(mat4 &M, vec3 &x, vec3 &y, vec3 &z)
{
	set(x, M.m_arr[0], M.m_arr[4], M.m_arr[ 8]);
	set(y, M.m_arr[1], M.m_arr[5], M.m_arr[ 9]);
	set(z, M.m_arr[2], M.m_arr[6], M.m_arr[10]);
}

mat4 transpose(const mat4 &M)
{
	return mat4(M[ 0], M[ 4], M[ 8], M[12],
				M[ 1], M[ 5], M[ 9], M[13],
				M[ 2], M[ 6], M[10], M[14],
				M[ 3], M[ 7], M[11], M[15]);
}

const mat4 translation(float x, float y, float z)
{
	return mat4(1, 0, 0, x,
				0, 1, 0, y,
				0, 0, 1, z,
				0, 0, 0, 1);
}

const mat4 translation(const vec3 &t)
{
	return translation(t.x, t.y, t.z);
}

void perspective(mat4 &M, float fov, float aspect, float zNear, float zFar)
{
	const float f = 1 / tanf(fov * RADIANS / 2),
		A = (zFar + zNear) / (zNear - zFar),
		B = (2 * zFar * zNear) / (zNear - zFar);

	M.m_arr[ 0] = f / aspect; M.m_arr[ 1] = 0; M.m_arr[ 2] = 0; M.m_arr[ 3] = 0; 
	M.m_arr[ 4] = 0;		  M.m_arr[ 5] = f; M.m_arr[ 6] = 0; M.m_arr[ 7] = 0; 
	M.m_arr[ 8] = 0;		  M.m_arr[ 9] = 0; M.m_arr[10] = A; M.m_arr[11] = B;
	M.m_arr[12] = 0;		  M.m_arr[13] = 0; M.m_arr[14] =-1; M.m_arr[15] = 0;
}

const mat4 perspective(float fov, float aspect, float zNear, float zFar)
{
	const float f = 1 / tanf(fov * RADIANS / 2),
		A = (zFar + zNear) / (zNear - zFar),
		B = (2 * zFar * zNear) / (zNear - zFar);

	return mat4(f / aspect, 0, 0, 0,
				         0, f, 0, 0,
				         0, 0, A, B,
				         0, 0,-1, 0);
}

void orthographic(mat4 &M, float left, float right, float bottom, float top, float zNear, float zFar)
{
	const float tx = - (right + left) / (right - left),
				ty = - (top + bottom) / (top - bottom),
				tz = - (zFar + zNear) / (zFar - zNear);

		M.m_arr[ 0] = 2/(right-left); M.m_arr[ 1] = 0;				M.m_arr[ 2] = 0;			  M.m_arr[ 3] = tx;
		M.m_arr[ 4] = 0;			  M.m_arr[ 5] = 2/(top-bottom); M.m_arr[ 6] = 0;				  M.m_arr[ 7] = ty;
		M.m_arr[ 8] = 0;			  M.m_arr[ 9] = 0;				M.m_arr[10] = -2/(zFar-zNear);M.m_arr[11] = tz;
		M.m_arr[12] = 0;			  M.m_arr[13] = 0;				M.m_arr[14] = 0;			  M.m_arr[15] = 1;
}

mat4 orthographic( float left, float right, float bottom, float top, float zNear, float zFar )
{
	const float tx = -(right + left) / (right - left),
		        ty = -(top + bottom) / (top - bottom),
		        tz = -(zFar + zNear) / (zFar - zNear);

	return mat4(2/(right-left),              0,              0, tx,
				             0, 2/(top-bottom),              0, ty,
	                         0,              0,-2/(zFar-zNear), tz,
	                         0,              0,              0, 1 );
}

// view mtrix
mat4 lookAt(const vec3 &position, const vec3 &center, const vec3 &up)
{
	const vec3 f = normalize(position - center);
	const vec3 s = normalize(cross(up, f));
	const vec3 u = normalize(cross(f, s));

	return mat4(s.x, s.y, s.z, -dot(s, position),
				u.x, u.y, u.z, -dot(u, position),
				f.x, f.y, f.z, -dot(f, position),
				0,	0,	0,	1);
}

const mat4 fromEuler(float x, float y, float z)
{
	const float cx = cosf(x * RADIANS), sx = sinf(x * RADIANS),
		cy = cosf(y * RADIANS), sy = sinf(y * RADIANS),
		cz = cosf(z * RADIANS), sz = sinf(z * RADIANS);

	// rotationX * rotationY * rotationZ
	return mat4(cy * cz, -cy * sz, sy, 0,
				cx * sz + sx * cz * sy, cx * cz - sx * sy * sz, -cy * sx, 0,
				sx * sz - cx * cz * sy, cz * sx + cx * sy * sz, cx * cy, 0,
				0, 0, 0, 1);
}

const mat4 fromEuler(const vec3 &r)
{
	return fromEuler(r.x, r.y, r.z);
}

const vec3 toEuler(const mat4 &M) 
{
	vec3 angle;
	float x, y, C;

	angle.y = asinf(M[2]);
	C       = cosf(angle.y);
	angle.y *=  DEGRESS;

	if (fabsf(C) > EPSILON)
	{
		x       =  M[10] / C;
		y       = -M[ 6] / C;
		angle.x = atan2f(y, x) * DEGRESS;
		x       =  M[ 0] / C;
		y       = -M[ 1] / C;
		angle.z = atan2f(y, x) * DEGRESS;
	} else
	{
		angle.x = 0.0f;
		x       =  M[ 5];
		y       =  M[ 4];
		angle.z = atan2f(y, x) * DEGRESS;
	}

	if (angle.x < 0) angle.x += 360;
	if (angle.y < 0) angle.y += 360;
	if (angle.z < 0) angle.z += 360;

	return angle;
}

//--------------------------
// Quaternion
//--------------------------


//-------------------------- 
// Misc
//-------------------------- 

template <typename T>
T bilerp(const T &a, const T &b, const T &c, const T &d, float u, float v)
{
	// Performs a bilinear interpolation.
	//  P(u,v) = e + v(f - e)
	//  
	//  where
	//  e = a + u(b - a)
	//  f = c + u(d - c)
	//  u in range [0,1]
	//  v in range [0,1]

	return a * ((1.0f - u) * (1.0f - v))
		+ b * (u * (1.0f - v))
		+ c * (v * (1.0f - u))
		+ d * (u * v);
}

void cartesianToSpherical(float x, float y, float z, float &rho, float &phi, float &theta)
{
	// All angles are in radians.
	// rho = distance from origin O to point P (i.e., length of OP)
	// phi = angle between OP and the XZ plane
	// theta = angle between X-axis and OP projected onto XZ plane

	rho = sqrtf((x * x) + (y * y) + (z * z));
	phi = asinf(y / rho);
	theta = atan2f(z, x);
}

bool  closeEnough(float f1, float f2)
{
	// Determines whether the two floating-point values f1 and f2 are
	// close enough together that they can be considered equal.

	return fabsf((f1 - f2) / ((f2 == 0.0f) ? 1.0f : f2)) < EPSILON;
}

float degreesToRadians(float degrees)
{
	return (degrees * PI) / 180.0f;
}

long floatToLong(float f)
{
	// Converts a floating point number into an integer.
	// Fractional values are truncated as in ANSI C.
	// About 5 to 6 times faster than a standard typecast to an integer.

	long fpBits = *reinterpret_cast<const long*>(&f);
	long shift = 23 - (((fpBits & 0x7fffffff) >> 23) - 127);
	long result = ((fpBits & 0x7fffff) | (1 << 23)) >> shift;

	result = (result ^ (fpBits >> 31)) - (fpBits >> 31);
	result &= ~((((fpBits & 0x7fffffff) >> 23) - 127) >> 31);

	return result;
}

bool isPower2(int x)
{
	return ((x > 0) && ((x & (x - 1)) == 0));
}

float lerp(float val, const float minValue, const float maxValue)
{
	if( val < minValue )
		return minValue;

	if( val > maxValue )
		return maxValue;

	return val;
}

int nextMultipleOf(int multiple, int value)
{
	// Returns the closest multiple of value that isn't less than value.

	return multiple * ((value + (multiple - 1)) / multiple);
}

int nextPower2(int x);

float radiansToDegrees(float radians)
{
	return (radians * 180.0f) / PI;
}

float random(float min, float max)
{
	// Returns a random number in range [min,max].

	return min + (max - min)
		* (static_cast<float>(rand()) / static_cast<float>(RAND_MAX));
}

void sphericalToCartesian(float rho, float phi, float theta, float &x, float &y, float &z)
{
	// All angles are in radians.
	// rho = distance from origin O to point P (i.e., length of OP)
	// phi = angle between OP and the XZ plane
	// theta = angle between X-axis and OP projected onto XZ plane

	x = rho * cosf(phi) * cosf(theta);
	y = rho * sinf(phi);
	z = rho * cosf(phi) * sinf(theta);
}

float smoothstep(float a, float b, float x)
{
	// Returns a gradual transition of 'x' from 0 to 1 beginning at
	// threshold 'a' and ending at threshold 'b'.
	//
	// References:
	//  [1] http://www.rendermanacademy.com/docs/smoothstep.htm
	//  [2] http://www.brlcad.org/doxygen/d8/d33/noise_8c-source.html
	//  [3] Natalya Tatarchuk, "Efficient High-Level Shader Development",
	//      Game Developers Conference Europe, August 2003.

	if (x < a)
	{
		return 0.0f;
	}
	else if (x >= b)
	{
		return 1.0f;
	}
	else
	{
		x = (x - a) / (b - a);
		return x * x * (3.0f - 2.0f * x);
	}
}